#include "FeatureFunctions.h"

#include <iostream>
#include <math.h>
#include "io.h"
#include "Config.h"
#include <map>

#define DEBUG

Descriptors getDescriptors(PamImage& inImage){
  Descriptors descriptors;
 
 // if(0 < Config::getInstance()->radonDistanceWeight){
    PamImage radonImage = getRadonTransform(inImage, Config::getInstance()->radonanglecount, Config::getInstance()->radonlinecount);
    descriptors.radonDescriptor = getRadonHistogram(radonImage, Config::getInstance()->radonhistowidth);
//  }
   
 // if(0 < Config::getInstance()->bottomProfileDistanceWeight){
    descriptors.bottomProfileDescriptor = getBottomProfile(inImage);
 // }
  
  descriptors.wordLength =  inImage.getWidth();  
  descriptors.heightWidthRatio = double(inImage.getWidth()) / double(inImage.getHeight());
  
  //if(0 < Config::getInstance()->lowResolutionWordDistanceWeight){
    descriptors.lowResolution = getLowResolution(inImage);
 // }
    
  return descriptors;
}

PamImage getRadonTransform(PamImage& inImage, int nAngles, int nLines)
{//zie wikipedia: http://en.wikipedia.org/wiki/Radon_transform
  //trace lines through image
  double minAngle = 0;     //radians
  double maxAngle = M_PI;  //radians
 
  long inWidth = inImage.getWidth();
  long inHeight = inImage.getHeight();
 
  long outWidth = nLines;   //nr of lines per angle
  long outHeight = nAngles; //nr of rotations
 
  double dAngle = (maxAngle - minAngle) / outHeight;
 
  long *values = new long[outHeight * outWidth];
  for(int i = 0; i < outHeight*outWidth; i++) values[i] = 0;
 
  double max_value = 0.0;
  //loop over input image
  for(int y = 0; y < inHeight; y++)
    for(int x = 0; x < inWidth; x++)
      //and rotate each pixel over an angle alpha
      for(int alpha_idx = minAngle; alpha_idx < outHeight; alpha_idx++)
      {
	// map alpha_idx in [0, outHeight) to alpha in [minAngle, maxAngle)
	double alpha = alpha_idx * dAngle + minAngle;
      
	// map input coordinates x,y into [x_min, x_max)
	// using a rotation and then a projection on horizontal axis
	double x_rotated = x * cos(alpha) - y * sin(alpha);
      
	// calculate extremes
	double x_min = -inHeight * sin(alpha) + (2 * alpha > M_PI ? inWidth * cos(alpha) : 0);
	double x_max = 2 * alpha > M_PI ? 0 : inWidth * cos(alpha);
	double mean = (x_max + x_min)/2;
	x_min = mean - outWidth/2.0;
	x_max = mean + outWidth/2.0;
      
	// map x_rotated in [x_min, x_max] to x_idx in [0, outWidth]
	int x_idx = floor((x_rotated - x_min) * outWidth / (x_max - x_min));
      
	if(0 <= x_idx && x_idx < outWidth)
	{
	  values[alpha_idx*outWidth + x_idx] += 255 - inImage.getPixelGray(x, y);
	  //values[alpha_idx*outWidth + x_idx] += inImage->getPixelGray(x, y); error
	  //track maximum for normalization
	  if(values[alpha_idx*outWidth + x_idx] > max_value)
	    max_value = values[alpha_idx*outWidth + x_idx];
	}
      }
  // copy normalized values
  PamImage outImage(GRAY_IMAGE, outWidth, outHeight);
  for(int y = 0; y < outHeight; y++)
    for(int x = 0; x < outWidth; x++)
      outImage.putPixel(x, y, (GrayPixel)(255.0 * values[y*outWidth + x] / max_value));
  return outImage;
}

vector<double> getRadonHistogram(PamImage& radonImage, int outWidth)
{
  // make histogram of image values for each angle
  int inWidth = radonImage.getWidth();
  int inHeight = radonImage.getHeight();
  
  std::vector<double> features(inWidth * outWidth);
  GrayPixel **inPixels = radonImage.getGrayPixels();
  for(int angle = 0; angle < inHeight; angle++)
    for(int offset = 0; offset < inWidth; offset++)
    {
      int x = outWidth * inPixels[angle][offset] / 256;
      if((inWidth * outWidth) <= (angle * outWidth + x)){
	cout << "OUT OF BOUNDS: Daniel, fix dit eens." << endl;
      }else{
	//TODO the index sometimes is out of bounds, dont know what should be happening here so Daniel can fix it
	//     is als het goed is gefixt nu.
	features[angle * outWidth + x] += 1.0 / inWidth;// = features[angle * outWidth + x} + 1;
      }
    }
  return features;
}

PamImage* lowResolutionToImage(vector<int> input){
  int maxValue = 0;
  for(unsigned i = 0; i < input.size(); i++){
    if(maxValue < input[i]) maxValue = input[i];
  }
  
  for(unsigned i = 0; i < input.size(); i++){
    input[i] = (input[i] * 255) / maxValue;
   // input[i] = 255 - input[i];
  }  
  int lrWidth = Config::getInstance()->lowResolutionWordWidth;
  int lrHeight = Config::getInstance()->lowResolutionWordHeight;
  
  int width = lrWidth * 30;
  int height = lrHeight * 30;
  
  PamImage* output = new PamImage(GRAY_IMAGE, width, height);
  for(int x = 0; x < width; x++){
    for(int y = 0; y < height; y++){
      output->putPixel( x,  y, input[(y/30)*lrWidth + (x/30)]);
    }
  }
  return output;
}

vector<int> getLowResolution(PamImage& wordImage){
  int lrWidth = Config::getInstance()->lowResolutionWordWidth;
  int lrHeight = Config::getInstance()->lowResolutionWordHeight;
  int lrTotalNum = lrWidth*lrHeight;
  vector<int> result;
  for(int i = 0; i < lrTotalNum; i++)result.push_back(0);
  int width = wordImage.getWidth();
  int height = wordImage.getHeight();
  GrayPixel** pixels = wordImage.getGrayPixels();
  for(int xIndex = 0; xIndex < width; xIndex++){
    for(int yIndex = 0; yIndex < height; yIndex++){
     if(pixels[yIndex][xIndex] == 0)continue;
     int x = int(floor((double(xIndex) / double(width)) * double(lrWidth)));
     int y = int(floor((double(yIndex) / double(height)) * double(lrHeight))); 
     int index = (y * lrWidth) + x;
     result[index]++;
    }
  }
  
  //TODO: eventueel normaliseren?
  return result;
}

double getLowResolutionDistance(vector<int> first, vector<int> second){
  if(first.size() != second.size()){
    cout << "size of low resolution differs, has the configuration changed between training and classifing?" << endl;
    throw new exception();
  }
  double distance = 0;
  for(unsigned i = 0; i < first.size(); i++){
    double localDistance = first[i] - second[i];
    if(Config::getInstance()->applyWeightsBeforeSquaring)localDistance *= Config::getInstance()->lowResolutionWordDistanceWeight;
    distance += (localDistance * localDistance);
  }
  distance /= first.size();
  return distance;
}

vector<int> getBottomProfile(PamImage& wordImage){
  vector<int> profile;
 // PamImage wordImage = word.image;
  int width = wordImage.getWidth();
  int height = wordImage.getHeight();
  GrayPixel** pixels = wordImage.getGrayPixels();
  int* samples = new int[width];
  for(int xIndex = 0; xIndex < width; xIndex++){
    for(samples[xIndex] = height - 1; 0 < samples[xIndex] && 0 < pixels[samples[xIndex]][xIndex]; samples[xIndex]--);
    if(samples[xIndex] == 0)samples[xIndex] = height/2;
  }
  /*#ifdef DEBUG
  PamImage output = *wordImage.convert(RGB_IMAGE);
  RGBPixel** rgbpixels = output.getRGBPixels();
  for(int xIndex = 0; xIndex < width; xIndex++){
    rgbpixels[samples[xIndex]][xIndex].r = 0;
    rgbpixels[samples[xIndex]][xIndex].g = 255;
    rgbpixels[samples[xIndex]][xIndex].b = 0;
  }
  SaveWordImage("BottomProfileDrawn", word, output);
  #endif*/
  double factor = double(width)/double(Config::getInstance()->bottomProfileSampleNumber);
  for(int i = 0; i < Config::getInstance()->bottomProfileSampleNumber; i++){
    profile.push_back(height - samples[int(factor * i)]);
  }
  delete [] samples;
  return profile;
}

double getBottomProfileDistance(vector<int> first, vector<int> second){
  if(first.size() != second.size()){
    cout << "size of bottom profile differs, has the configuration changed between training and classifing? "<< first.size() << " and " <<second.size() << endl;
    throw new exception();
  }
  double distance = 0;
  for(unsigned i = 0; i < first.size(); i++){
   // cout << "(" << first[i] <<", "<< second[i] << ") ";
    double localDistance = first[i] - second[i];
    //cout << localDistance << " ";
    if(Config::getInstance()->applyWeightsBeforeSquaring)localDistance *= Config::getInstance()->bottomProfileDistanceWeight;
    distance += (localDistance * localDistance);
  }
 // cout << distance << endl;
  distance /= first.size();
 // cout << distance << endl;
  return distance;
}

/*double distance(Descriptor vec1, Descriptor vec2)
{
 //TODO: implement 
}*/

double getHeightWidthRatioDistance(double first, double second){
   if(first < 0 && second < 0){
     first = 1 / first;
     second = 1 / second;
   }
   double distance = first - second;
   if(Config::getInstance()->applyWeightsBeforeSquaring)distance *= Config::getInstance()->heightWidthRatioWeight;
   return distance * distance;
}

double featureDistance(Descriptor featureVec1, Descriptor featureVec2)
{
  if(featureVec1.size() != featureVec2.size()){
    cout << "different sizes of descriptor: " << featureVec1.size() << " and "<< featureVec2.size() << endl;
    throw new exception();
  }
  double distance = 0.0;
  for(unsigned i = 0; i < featureVec1.size(); i++){
    double localDistance = featureVec1[i] - featureVec2[i];
    if(Config::getInstance()->applyWeightsBeforeSquaring)localDistance *= Config::getInstance()->radonDistanceWeight;
    distance += (localDistance * localDistance);
  }
  distance /= featureVec1.size();
  return distance;
}


vector<Word> createLineWithLowestScore(vector<Word> inputWords){

  priority_queue<Word*, vector<Word*>, WordPositionCompare> orderedWords;
  for(unsigned i = 0; i < inputWords.size(); i++){
    if(inputWords[i].score == -1)continue;
    orderedWords.push(&inputWords[i]);
  }  
  
  if(orderedWords.size() ==0){
    return inputWords;
  }
  vector<Word*> previousWords;
  
  vector<vector<Word*> > bestlines = doCreateLineWithLowestScore(orderedWords, previousWords); 
  if(bestlines.size() > 1){
    cout << endl << endl << "ERROR MULTIPLE BEST LINES: returning inputWords";
    for(unsigned i = 0; i < bestlines.size(); i++){
      cout << endl;
       for(unsigned j = 0; j < bestlines[i].size(); j++){
	cout << bestlines[i][j]->text << "  ";
      }
    }
    cout << endl << endl;
    return inputWords;
  }
  if(bestlines.size() == 0){
    cout << endl << endl << "ERROR NO BEST LINES: returning inputWords" << endl;
    return inputWords;
  }
  vector<Word> result;
  for(unsigned i = 0; i < bestlines[0].size(); i++){
    result.push_back(*bestlines[0][i]);
  }
  cout << endl << endl << "BEST LINE:" << endl;
  for(unsigned i = 0; i < result.size(); i++){
     cout << result[i].text << "  ";
  }
  cout << endl << endl;
  return result;
}
//***************************************************************************************
vector<Word> ToWordVector(WordAStarNode* line){
  vector<Word> result;
  WordAStarNode* currentWord = line; 
  while(currentWord != NULL){
    result.push_back(*currentWord->word);
    currentWord = currentWord->previousNode;
  } 
  return result;
}

class WordAStarCompare
{
  public:
  bool operator() (const WordAStarNode* lhs, const WordAStarNode* rhs) const {
    return lhs->fScore > rhs->fScore;
  }
};

vector<Word> GetBestLine(vector<Word> words){  
  priority_queue<WordAStarNode*, vector<WordAStarNode*>, WordAStarCompare> openSet;
  double smallestLeft = words[0].left;
  double biggestRight = words[0].right;
  
  for(unsigned i = 0; i < words.size(); i++){
    Word* current = &words[i];
    if(current->left < smallestLeft)smallestLeft = current->left;
    if(biggestRight < current->right)biggestRight = current->right;
  }
  
  
  //initialize nodes
  vector<WordAStarNode*> nodes;  
  for(unsigned i = 0; i < words.size(); i++){
    WordAStarNode* node = new WordAStarNode();
    node->word = &words[i];
    node->isClosed = false;
    node->isOpen = false;
    node->previousNode = NULL;
    node->weight = node->word->score * node->word->width();
    if(node->weight < node->word->width())node->weight = node->word->width();
    node->isFinish = (abs(node->word->right - biggestRight)<50);
    nodes.push_back(node);
    if(abs(node->word->left - smallestLeft)<50){
      node->isOpen = true;
      node->gScore = node->weight;//0;
      node->hScore = biggestRight - node->word->right;
      node->fScore = node->hScore;
      openSet.push(node);
    }
  }
  
   
  //create adjecend map
  map<WordAStarNode*, vector<WordAStarNode*> > wordGraph; 
  for(unsigned i = 0; i < nodes.size(); i++){
    vector<WordAStarNode*> adjecendWords;
    WordAStarNode* currentWord = nodes[i];
    for(unsigned j = 0; j  < nodes.size(); j++){
      WordAStarNode* adjecendWord = nodes[j];
      if(50 < abs(double(currentWord->word->right - adjecendWord->word->left)))continue;
      adjecendWords.push_back(adjecendWord);
    }
    wordGraph.insert(pair<WordAStarNode*, vector<WordAStarNode*> >(currentWord, adjecendWords));
  }

  vector<Word> result = words;
  while(0 < openSet.size()){
    //get the node with the lowest fscore
    WordAStarNode* currentNode = openSet.top();
    openSet.pop();
    
    currentNode->isClosed = true;
    currentNode->isOpen = false;
    
    if(currentNode->isFinish){
      //cout << "FOUND A PATH!" << endl;
      result = ToWordVector(currentNode);
      break;
    } 
    
    //for all neighbournodes
    

    map<WordAStarNode*, vector<WordAStarNode*> >::iterator  it = wordGraph.find(currentNode);

    if(it != wordGraph.end()){
      vector<WordAStarNode*> adjecendNodes = it->second;    

	for(unsigned i = 0; i < adjecendNodes.size(); i++){      
	
	  WordAStarNode* adjecendNode = adjecendNodes[i];  
	  if(!adjecendNode->isClosed){	
	    double tentativeGScore = currentNode->gScore + adjecendNode->weight;
	    if(!adjecendNode->isOpen || tentativeGScore < adjecendNode->gScore){
	      adjecendNode->previousNode = currentNode;
	      adjecendNode->gScore = tentativeGScore;
	      adjecendNode->hScore = biggestRight - adjecendNode->word->right;
	      adjecendNode->fScore = adjecendNode->gScore + adjecendNode->hScore;
	      adjecendNode->isClosed = false;
	    }
	    if(!adjecendNode->isOpen){  
	      openSet.push(adjecendNode);
	      adjecendNode->isOpen = true;
	    }
	  }
	}   
    }else{
      cout << "MAPPING NOT FOUND" << endl;
    }
  }  
  
  //free memory 
  for(unsigned i = 0; i < nodes.size(); i++){
    delete  nodes[i];
  }
  return result;
}


//***************************************************************************************
vector<vector<Word*> > doCreateLineWithLowestScore(priority_queue<Word*, vector<Word*>, WordPositionCompare> orderedWords, vector<Word*> previousWords){  
  vector<vector<Word*> > result;
  vector<Word*> currentWords = previousWords;
    Word* word = orderedWords.top();
  
    
    orderedWords.pop();
    if(orderedWords.size() == 0){     
      vector<Word*> end;
      end.push_back(word);
      result.push_back(end);
      return result;
    }  
    currentWords.push_back(word);
    vector<vector<Word*> > potentialTails = doCreateLineWithLowestScore(orderedWords, currentWords);
   /* cout << endl << endl <<"CURRENT WORD:" << endl;
    cout << word->text;
   
    cout << endl << endl <<"PREVIOUS WORDS:" << endl;
    for(unsigned i = 0; i < previousWords.size(); i++){
	cout << previousWords[i]->text << "  ";
    }
    
    cout << endl << endl <<"POTENTIAL TAILS:";
    for(unsigned potentialTailIndex = 0; potentialTailIndex < potentialTails.size(); potentialTailIndex++){
      cout << endl;
       for(unsigned i = 0; i < potentialTails[potentialTailIndex].size(); i++){
	cout << potentialTails[potentialTailIndex][i]->text << "  ";
      }
    }
    cout << endl << endl;*/
    
    result = MergeWordToPotentialTails(potentialTails, word, previousWords, false);
    if(result.size() == 0){
      result = MergeWordToPotentialTails(potentialTails, word, previousWords, true);
    }
    if(100 < result.size()){
      cout << "To many potential tails, this will slow things down dramatically, ditch some and hope we get the right one :)" << endl;
      return GetOnlyBestScoringLine(result, word->left);
    }
    if(overlaps(previousWords, word))return result;
    return GetOnlyBestScoringLine(result, word->left);
 
}

vector<vector<Word*> > MergeWordToPotentialTails(vector<vector<Word*> > potentialTails, Word* word, vector<Word*> previousWords, bool ignoreMaxGap){
  vector<vector<Word*> > result;
  for(unsigned potentialTailIndex = 0; potentialTailIndex < potentialTails.size(); potentialTailIndex++){
    unsigned overlapNum = 0;
    double overlapScore = 0;
  
    for(unsigned i = 0; i < potentialTails[potentialTailIndex].size() && potentialTails[potentialTailIndex][i]->overlapsWithMargin(word, Config::getInstance()->maxGap); i++){
      overlapNum ++;	
      overlapScore += potentialTails[potentialTailIndex][i]->score;
    }
    if(overlapNum != 0) overlapScore /= overlapNum;    
    
    if((overlapNum == 0 || word->score < overlapScore) && 
      (ignoreMaxGap || overlapNum == potentialTails[potentialTailIndex].size() 
         || (potentialTails[potentialTailIndex][overlapNum]->left - word->right) < Config::getInstance()->maxGap)){
      vector<Word*> localResult;
      localResult.push_back(word);
      for(unsigned i = overlapNum; i < potentialTails[potentialTailIndex].size(); i++){	   
	localResult.push_back(potentialTails[potentialTailIndex][i]);
      }
      result.push_back(localResult);
    }        
    
    if(overlapNum != 0 && (overlaps(previousWords, potentialTails[potentialTailIndex][0]) || overlapScore < word->score)){
      result.push_back(potentialTails[potentialTailIndex]);
    }
  }
  return result;
}

vector<vector<Word*> > GetOnlyBestScoringLine(vector<vector<Word*> > input, int start){  
    vector<vector<Word*> > validResults;
    for(unsigned i = 0; i < input.size(); i++){
      if(input[i].size() == 0 || Config::getInstance()->maxGap < (start - input[i][0]->right))continue;
      validResults.push_back(input[i]);
    }
    
   // cout << "get best score";
    if(validResults.size() < 2)return validResults;
    vector<Word*> bestResult = validResults[0];
    double bestScore = score(validResults[0]);
   // cout << "score: "<< bestScore << endl;
    for(unsigned i = 1; i < validResults.size(); i++){
      double currentScore = score(input[i]);
     // cout << "score: "<< currentScore << endl;
      if(bestScore < currentScore)continue;
      bestScore = currentScore;
      bestResult = validResults[i];
    }
    vector<vector<Word*> > result;
    result.push_back(bestResult);
    return result;
}

bool overlaps(vector<Word*> words, Word* word){
  for(unsigned i = 0; i < words.size(); i++){
    if(words[i]->overlapsWithMargin(word, Config::getInstance()->maxGap))return true;
  }
  return false;
}

double score(vector<Word*> words){
  double score = 0;
  for(unsigned i = 0; i < words.size(); i++) score += words[i]->score;
  return score;
}


















